last edits:
Requires IPython to run in "pylab" mode. If this is not the case, at least insert
from pylab import *
somewhere
This script tries to identify a minimalistic SLIP extension (e.g., Ankle-Y-position only).
Requirements:
In [105]:
# define and import data
subject = 2 #available subjects: 1,2,3,7. Other subjects have comparatively bad data quality
ttype = 1 # 1: free running, 2: metronome running (data incomplete!)
n_factors = 5 # 1-5 factors to predict SLIP parameters
exclude_IC_from_factors = False # exclude the initial conditions from kinematic state
#prior to identifying SLIP parameter predictors?
In [106]:
import mutils.io as mio
import mutils.misc as mi
import os
import re
# load kinematic data
k = mio.KinData()
k.load(subject, ttype)
k.selection = ['r_anl_y - com_y', 'l_anl_y - com_y', 'com_z']
SlipData = [mi.Struct(mio.mload('../data/2013-newSlip/SLIP_s%it%ir%i.dict' % (subject, ttype, rep)))
for rep in k.reps]
indices_right = [mi.upper_phases(d.phases[:-1], sep=0, return_indices=True) for d in SlipData]
indices_left = [mi.upper_phases(d.phases[:-1], sep=pi, return_indices=True) for d in SlipData]
starts_right = [idxr[0] < idxl[0] for idxr, idxl in zip(indices_right, indices_left)]
param_right = [ vstack(d.P)[idxr, :] for d, idxr in zip(SlipData, indices_right)]
param_left = [ vstack(d.P)[idxl, :] for d, idxl in zip(SlipData, indices_left)]
IC_right = [ vstack(d.IC)[idxr, :] for d, idxr in zip(SlipData, indices_right)]
IC_left = [ vstack(d.IC)[idxl, :] for d, idxl in zip(SlipData, indices_left)]
In [107]:
#select any data
import models.sliputil as su
dat = SlipData[2]
stepnr = 32
print "simulation:", su.finalState(dat.IC[stepnr], dat.P[stepnr], {'m' : dat.mass, 'g': dat.g})
print '==================='
print "experiment:", dat.IC[stepnr + 1]
In [108]:
# find minimal predictors
import mutils.statistics as st
import mutils.FDatAn as fda
# set apex data
# *NOTE* the first three labels have to be "com_x", "com_y", "com_z". If you edit this, make sure to edit the cell where data
# are further processed...
k.selection = [ 'com_x', 'com_y', 'com_z',
'r_anl_y - com_y', 'r_anl_x - com_x', 'r_mtv_z - r_anl_z', 'r_mtv_x - r_anl_x', 'r_kne_y - com_y',
'r_elb_y - com_y', 'r_elb_x - com_x', 'r_wrl_z - com_z', 'r_wrl_x - com_x', 'cvii_y - com_y',
'l_anl_y - com_y', 'l_anl_x - com_x', 'l_mtv_z - l_anl_z', 'l_mtv_x - l_anl_x', 'l_kne_y - com_y',
'l_elb_y - com_y', 'l_elb_x - com_x', 'l_wrl_z - com_z', 'l_wrl_x - com_x', ]
# sep = 0 -> right leg will touchdown next
# sep = pi -> right leg will touchdown next
# always exclude last apex - there are no SLIP parameters defined for it!
kin_right = k.get_kin_apex( [mi.upper_phases(d.phases[:-1], sep=0) for d in SlipData],)
kin_left = k.get_kin_apex( [mi.upper_phases(d.phases[:-1], sep=pi) for d in SlipData],)
In [109]:
# build common dataset
all_kin_r = []
all_kin_l = []
all_param_r = []
all_param_l = []
all_IC_r = []
all_IC_l = []
for rep in arange(len(starts_right)): #kr, kl, sr in zip(kin_right, kin_left, starts_right):
# when repetition starts with right step: select
kin_r = vstack(kin_right[rep]).T
kin_l = vstack(kin_left[rep]).T
par_r = param_right[rep]
par_l = param_left[rep]
IC_r = IC_right[rep]
IC_l = IC_left[rep]
if not starts_right[rep]:
# omit first value in kin_l!
kin_l = kin_l[1:, :]
par_l = par_l[1:, :]
IC_l = IC_l[1:, :]
minlen = min(kin_r.shape[0], kin_l.shape[0])
kin_r = hstack([kin_r[:minlen, 2 : len(k.selection) + 2] ,
kin_r[:minlen, len(k.selection) + 3 :]])# remove absolute position + vertical velocity
kin_l = hstack([kin_l[:minlen, 2 : len(k.selection) + 2] ,
kin_l[:minlen, len(k.selection) + 3 :]])# remove absolute position + vertical velocity
par_r = par_r[:minlen, :]
par_l = par_l[:minlen, :]
IC_r = IC_r[:minlen, :]
IC_l = IC_l[:minlen, :]
all_IC_r.append(IC_r)
all_IC_l.append(IC_l)
all_param_r.append(par_r)
all_param_l.append(par_l)
all_kin_r.append(kin_r)
all_kin_l.append(kin_l)
all_IC_r = vstack(all_IC_r)
all_IC_l = vstack(all_IC_l)
all_param_r = vstack(all_param_r)
all_param_l = vstack(all_param_l)
all_kin_r = vstack(all_kin_r)
all_kin_l = vstack(all_kin_l)
In [109]:
In [110]:
stepnr = 1299
mass = mean([d.mass for d in SlipData])
# su.finalState actually performs a one-step integration of SLIP
print "simres [right step]:", su.finalState(all_IC_r[stepnr, :], all_param_r[stepnr, :], {'m' : mass, 'g' : -9.81})
print "subsequent left apex:", all_IC_l[stepnr, :]
print "==========="
print "simres [left step]:", su.finalState(all_IC_l[stepnr, :], all_param_l[stepnr, :], {'m' : mass, 'g' : -9.81})
print "subsequent right apex:", all_IC_r[stepnr + 1, :]
In [111]:
# detrend - look at residuals!
dt_kin_r = fda.dt_movingavg(all_kin_r, 30)
dt_kin_l = fda.dt_movingavg(all_kin_l, 30)
dt_param_r = fda.dt_movingavg(all_param_r, 30)
dt_param_l = fda.dt_movingavg(all_param_l, 30)
dt_IC_r = fda.dt_movingavg(all_IC_r, 30)
dt_IC_l = fda.dt_movingavg(all_IC_l, 30)
# use the *same* values for normalization for left and right!
# yes, it's 'sdt', not 'std': "Scores of DeTrented" ...
sdt_kin_r = dt_kin_r / std(dt_kin_r, axis=0)
sdt_kin_l = dt_kin_l / std(dt_kin_r, axis=0)
sdt_param_r = dt_param_r / std(dt_param_r, axis=0)
sdt_param_l = dt_param_l / std(dt_param_r, axis=0)
sdt_kin_r -= mean(sdt_kin_r, axis=0)
sdt_kin_l -= mean(sdt_kin_l, axis=0)
sdt_param_r -= mean(sdt_param_r, axis=0)
sdt_param_l -= mean(sdt_param_l, axis=0)
sdt_kin_r_noIC = hstack([sdt_kin_r[:,1:20],sdt_kin_r[:, 22:]])
sdt_kin_l_noIC = hstack([sdt_kin_l[:,1:20],sdt_kin_l[:, 22:]])
In [112]:
print dt_kin_r.shape, dt_param_r.shape
print dt_kin_l.shape, dt_param_l.shape
In [113]:
# parameter to predict. select [0,1,2,4] for 2D-SLIP (excluding beta), or [0,1,2,3,4] for full 3D SLIP
p2D_r = sdt_param_r[:, [0,1,2,3,4]]
p2D_l = sdt_param_l[:, [0,1,2,3,4]]
In [114]:
# Here, the main predictors ("directions") are computed. This is computationally quite fast
if exclude_IC_from_factors:
facs_r = st.find_factors(sdt_kin_r_noIC.T, p2D_r.T, k=n_factors)
facs_l = st.find_factors(sdt_kin_l_noIC.T, p2D_l.T, k=n_factors)
fscore_r = dot(facs_r.T, sdt_kin_r_noIC.T).T
fscore_l = dot(facs_l.T, sdt_kin_l_noIC.T).T
else:
facs_r = st.find_factors(sdt_kin_r.T, p2D_r.T, k=n_factors)
facs_l = st.find_factors(sdt_kin_l.T, p2D_l.T, k=n_factors)
# project data on lower dimensional subspace (do not take to full space again)
fscore_r = dot(facs_r.T, sdt_kin_r.T).T
fscore_l = dot(facs_l.T, sdt_kin_l.T).T
In [115]:
# reduced model: 6D state, 3 predictors
#reddat_r = hstack([fscore_r, dt_IC_r])
#reddat_l = hstack([fscore_l, dt_IC_l])
# form a model that *only* conists of parts of the "full" data (*excatly* the same data),
# which however may be projected to some axes
IC_r_from_dt = sdt_kin_r[:,[0, 20, 21]] # CoM height and horizontal plane velocities
IC_l_from_dt = sdt_kin_l[:,[0, 20, 21]]
reddat_r = hstack([fscore_r, IC_r_from_dt])
reddat_l = hstack([fscore_l, IC_l_from_dt])
# reddat_r = fscore_r
# reddat_l = fscore_l
In [116]:
# "select the initial conditions from full state information" - matrix
# this matrix only makes sense if exclude_IC_from_factors is set to False!
if not exclude_IC_from_factors:
IC_sel = zeros((41, 3))
IC_sel[0, 0] = 1
IC_sel[20, 1] = 1
IC_sel[21, 2] = 1
rm_selector_r = hstack([facs_r, IC_sel])
rm_selector_l = hstack([facs_l, IC_sel])
In [117]:
# manually perform prediction. Here: in-sample!
reload(st)
idat1 = sdt_kin_r
if not exclude_IC_from_factors:
idat2 = dot(rm_selector_r.T, sdt_kin_r.T).T
else:
idat2 = reddat_r
odat = p2D_r
print "Data is the same as 'original' reduced model:", allclose(idat2, reddat_r)
# perform regression:
vred1 = []
vred2 = []
vred1c = []
vred2c = []
for rep in range(50):
sel_idx = randint(0, odat.shape[0], odat.shape[0])
# test: remove double indices (this is done internally in vRedPartial)
# -> then, vRedPartial will return identical results
sel_idx = array(list(set(sel_idx)))
A1 = lstsq(idat1[sel_idx, :], odat[sel_idx, :])[0]
A2 = lstsq(idat2[sel_idx, :], odat[sel_idx, :])[0]
# test
#A1b = dot(pinv(idat1[sel_idx, :]), odat[sel_idx, :])
#A2b = dot(pinv(idat1[sel_idx, :]), odat[sel_idx, :])
#print "computation as expected:", allclose(A1, A1b), allclose(A1, A1b)
# compute predictions
pred1 = dot(idat1[sel_idx, :], A1)
pred2 = dot(idat2[sel_idx, :], A2)
# compute relative remaining variance after prediction
rvar1 = var(odat[sel_idx, :] - pred1, axis=0) / var(odat[sel_idx, :], axis=0)
rvar2 = var(odat[sel_idx, :] - pred2, axis=0) / var(odat[sel_idx, :], axis=0)
rvar1c = fda.vRedPartial(idat1, odat, [A1, ], [fda.otheridx(sel_idx, odat.shape[0]), ], vaxis=0)[0]
rvar2c = fda.vRedPartial(idat2, odat, [A2, ], [fda.otheridx(sel_idx, odat.shape[0]), ], vaxis=0)[0]
# store results
vred1.append(rvar1)
vred2.append(rvar2)
vred1c.append(rvar1c)
vred2c.append(rvar2c)
vred1b = st.predTest(idat1, odat, out_of_sample=False, rcond=1e-7)
vred2b = st.predTest(idat2, odat, out_of_sample=False, rcond=1e-7)
In [118]:
#visualize
figure()
b1 = boxplot(vstack(vred1), positions = arange(odat.shape[1]) + 1, widths=.15)
b2 = boxplot(vstack(vred2), positions = arange(odat.shape[1]) + 1.1, widths=.15)
#b1b = boxplot(vstack(vred1c), positions = arange(odat.shape[1]) + 1.3, widths=.15)
#b2b = boxplot(vstack(vred2c), positions = arange(odat.shape[1]) + 1.4, widths=.15)
b1b = boxplot(vstack(vred1b), positions = arange(odat.shape[1]) + 1.3)
b2b = boxplot(vstack(vred2b), positions = arange(odat.shape[1]) + 1.4)
mi.recolor(b1, 'k')
mi.recolor(b2, 'r')
mi.recolor(b1b, 'b')
mi.recolor(b2b, 'm')
xlim(0, 6)
ylim(.4,.45)
draw()
In [119]:
predict_oos = True
# select reduced models as a real subset of the full kinematic state at apex
if not exclude_IC_from_factors:
# use a real projection of the data!
reddat_r = dot(rm_selector_r.T, sdt_kin_r.T).T
reddat_l = dot(rm_selector_l.T, sdt_kin_l.T).T
#p2D_r = p2D_r - mean(p2D_r, axis=0)
#sdt_kin_r = sdt_kin_r - mean(sdt_kin_r, axis=0)
res1 = st.predTest(reddat_r, p2D_r, out_of_sample=predict_oos, rcond=1e-7)
res2 = st.predTest(sdt_kin_r, p2D_r, out_of_sample=predict_oos, rcond=1e-7)
res3 = st.predTest(reddat_l, p2D_l, out_of_sample=predict_oos, rcond=1e-7)
res4 = st.predTest(sdt_kin_l, p2D_l, out_of_sample=predict_oos, rcond=1e-7)
figure(figsize=(12,6))
subplot(1,2,1)
b1 = boxplot(res1)
b2 = boxplot(res2)
mi.recolor(b1, 'r')
mi.recolor(b2, 'k')
xticks(arange(5) + 1)
gca().set_xticklabels(['k', r'$\alpha$', r'L$_0$', r'$\beta$', r'$\delta$E'], fontsize=16)
xlabel('predicted variable')
ylabel('relative remaining variance')
ylim(0, 1.05)
title('right step SLIP params\nred: reduced model,\nblack: full kinematic state')
subplot(1,2,2)
b1 = boxplot(res3)
b2 = boxplot(res4)
mi.recolor(b1, 'r')
mi.recolor(b2, 'k')
xticks(arange(5) + 1)
gca().set_xticklabels(['k', r'$\alpha$', r'L$_0$', r'$\beta$', r'$\delta$E'], fontsize=16)
xlabel('predicted variable')
ylabel('relative remaining variance')
ylim(0, 1.05)
title('left step SLIP params\nred: reduced model,\nblack: full kinematic state')
draw()
In [120]:
# test self-consistency
oos_pred = True # out of sample prediction?
# predict
res1 = st.predTest(reddat_r, reddat_l, out_of_sample=oos_pred, rcond=1e-7)
res2 = st.predTest(sdt_kin_r, reddat_l, out_of_sample=oos_pred, rcond=1e-7)
res3 = st.predTest(reddat_l[:-1, :], reddat_r[1:, :], out_of_sample=oos_pred, rcond=1e-7)
res4 = st.predTest(sdt_kin_l[:-1, :], reddat_r[1:, :], out_of_sample=oos_pred, rcond=1e-7)
figure(figsize=(16,8))
subplot(1,2,1)
b1 = boxplot(res1)
b2 = boxplot(res2)
mi.recolor(b1, 'r')
mi.recolor(b2, 'k')
ylim(0, 1.05)
xticks(arange(res3.shape[1]) + 1)
gca().set_xticklabels(['factor ' + str(nr + 1) for nr in arange(res1.shape[1] - 3)] +
['height', 'horiz. speed', 'lat. speed'], rotation=45)
title('predicting left apex state\nred: reduced model, black: full model')
ylabel('relative remaining variance')
subplot(1,2,2)
b1 = boxplot(res3)
b2 = boxplot(res4)
mi.recolor(b1, 'r')
mi.recolor(b2, 'k')
ylim(0, 1.05)
xticks(arange(res3.shape[1]) + 1)
gca().set_xticklabels(['factor ' + str(nr + 1) for nr in arange(res1.shape[1] - 3)] +
['height', 'horiz. speed', 'lat. speed'], rotation=45)
title('predicting left apex state\nred: reduced model, black: full model')
ylabel('relative remaining variance')
draw()
In [121]:
# eigenvalue analyis
# full model
_, maps_1, _ = fda.fitData(sdt_kin_r, sdt_kin_l, nps=1, nrep=200, rcond=1e-7)
_, maps_2, _ = fda.fitData(sdt_kin_l[:-1, :], sdt_kin_r[1:, :], nps=1, nrep=200, rcond=1e-7, )
maps_full = [dot(m1, m2) for m1, m2 in zip(maps_1, maps_2)]
# reduced model
_, maps_1, _ = fda.fitData(reddat_r, reddat_l, nps=1, nrep=200, rcond=1e-7)
_, maps_2, _ = fda.fitData(reddat_l[:-1, :], reddat_r[1:, :], nps=1, nrep=200, rcond=1e-7, )
maps_red = [dot(m1, m2) for m1, m2 in zip(maps_1, maps_2)]
In [122]:
import fmalibs.util as ut
vi_full = ut.VisEig(127, False)
for A in maps_full:
vi_full.add(eig(A)[0])
vi_red = ut.VisEig(127, False)
for A in maps_red:
vi_red.add(eig(A)[0])
figure(figsize=(18, 9))
subplot(1,2,1)
vi_full.vis1()
xlim(-1,1)
ylim(-1,1)
title('eigenvalue distribution "full model"')
subplot(1,2,2)
vi_red.vis1()
vi_full.vis1()
xlim(-1,1)
ylim(-1,1)
title('eigenvalue distribution "reduced model"')
Out[122]:
In [123]:
nps = 30 # number of sections per stride
map_sections = [0, 8, 15, 23] # select some sections for which mappings should be computed
map_sections = [0, 5, 10, 15, 20, 25] # select some sections for which mappings should be computed
#map_sections = [0, 7, 13, 19,25] # select some sections for which mappings should be computed
#map_sections = [0, 10, 20] # select some sections for which mappings should be computed
k_n = fda.dt_movingavg(k.make_1D(nps=nps)[:, 2 * nps:], 30) # exclude horizontal CoM position
k_n -= mean(k_n, axis=0)
k_n /= std(k_n, axis=0)
maps_pt, maps_full, idcs = fda.fitData(k_n[:-1, :], k_n[1:, :], nps=nps, sections=map_sections, nrep=200, rcond=1e-7 )
In [124]:
# compute stride maps from sections maps
# note: the correct order in the product of maps is map_n * map_(n-1) * ... * map_1
# otherwise, the resulting matrix is not a propagator
# -> reverse ordering of maps
all_maps = [reduce(dot, maps[::-1]) for maps in zip(*maps_pt)]
vi3 = ut.VisEig(127, False)
for A in all_maps:
vi3.add(eig(A)[0])
figure(figsize=(18, 6))
subplot(1,3,1)
vr = vi_full.vis1()[0]
vr.set_cmap(cm.jet)
xlim(-1,1)
ylim(-1,1)
title('eigenvalue distribution "full model"')
subplot(1,3,2)
vr = vi_full.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
xlim(-1,1)
ylim(-1,1)
title('eigenvalue distribution "reduced model" (red)\n vs. full model (sections at apex)')
subplot(1,3,3)
vr = vi3.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
xlim(-1,1)
ylim(-1,1)
title('eigenvalues of full kinematic model\nwith %i sections per stride\n vs. reduced model(red)'
% len(map_sections))
xlabel('real part')
ylabel('imaginary part')
draw()
Hypothesis: There should be almost no difference according to results from my thesis.
The closed loop model reads as follows:
Step 1 ("right"):
state = [IC; Factors] where IC = initial CoM state at apex
params = P1 * state where P is a regression from data
new_state = [ode(IC, params); Q1 * [IC; Factors]] where Q is a regressed map from data
Step 2 ("left"):
state = [IC; Factors] where IC = initial CoM state at apex
params = P2 * state where P is a regression from data
new_state = [ode(IC, params); Q2 * [IC; Factors]] where Q is a regressed map from data
In [125]:
mean_pr = mean(vstack(param_right), axis=0)
mean_pl = mean(vstack(param_left), axis=0)
mean_ICr = mean(all_IC_r, axis=0)
mean_ICl = mean(all_IC_l, axis=0)
mean_pl[4] = -mean_pr[4] # set total energy change to exactly zero.
# Note: typically, the difference is low, roughly ~0.01 J
ICp_r, ICp_l = su.getPeriodicOrbit_p(mean_pr, mean_pl, aug_dict={'m': mass, 'g' : -9.81}, startState=mean_ICr)
In [126]:
#verify periodicity
sr1 = su.finalState(ICp_r, mean_pr, addDict = {'m' : mass, 'g' : -9.81})
sl1 = su.finalState(ICp_l, mean_pl, addDict = {'m' : mass, 'g' : -9.81})
print "difference between identified periodic orbit and simulation results:"
print sr1 - ICp_l
print sl1 - ICp_r
In [127]:
print "Deviation from periodic orbit to average apex state, in units of standard deviations:"
print " [height, horizontal speed, lateral speed]"
print "left step: ", (ICp_l - mean_ICl) / std(all_IC_l, axis=0)
print "right step:", (ICp_r - mean_ICr) / std(all_IC_r, axis=0)
In [128]:
print dt_kin_r.shape, dt_param_r.shape
print dt_kin_l.shape, dt_param_l.shape
In [128]:
In [129]:
dt_fstate_r = hstack([fda.dt_movingavg(all_IC_r, 30), fscore_r])
dt_fstate_l = hstack([fda.dt_movingavg(all_IC_l, 30), fscore_l])
dt_fstate_r -= mean(dt_fstate_r, axis=0)
dt_fstate_l -= mean(dt_fstate_l, axis=0)
#dt_param_r = fda.dt_movingavg(vstack(param_right), 30)
#dt_param_l = fda.dt_movingavg(vstack(param_left), 30)
#dt_param_r -= mean(dt_param_r, axis=0)
#dt_param_l -= mean(dt_param_l, axis=0)
# compute parameter prediction maps
_, all_ctrl_r, _ = fda.fitData(dt_fstate_r, dt_param_r, nps=1, nrep=200, sections=[0,], rcond=1e-8)
_, all_ctrl_l, _ = fda.fitData(dt_fstate_l, dt_param_l, nps=1, nrep=200, sections=[0,], rcond=1e-8)
ctrl_r = fda.meanMat(all_ctrl_r)
ctrl_l = fda.meanMat(all_ctrl_l)
# compute factors prediction maps - "propagators" of factor's state
_, all_facprop_r, _ = fda.fitData(dt_fstate_r, fscore_l, nps=1, nrep=200, sections=[0,], rcond=1e-8)
_, all_facprop_l, _ = fda.fitData(dt_fstate_l[:-1, :], fscore_r[1:, :], nps=1, nrep=200, sections=[0,], rcond=1e-8)
facprop_r = fda.meanMat(all_facprop_r)
facprop_l = fda.meanMat(all_facprop_l)
In [130]:
std(array(all_facprop_l), axis=0)
figure()
subplot(1,2,1), pcolor(mean(array(all_facprop_l), axis=0)), colorbar()
subplot(1,2,2), pcolor(std(array(all_facprop_l), axis=0)), colorbar()
Out[130]:
In [131]:
print mean(st.predTest(dt_fstate_l[:-1,:], fscore_r[1:, :], totvar=False, out_of_sample=True), axis=0)
print mean(st.predTest(dt_fstate_r, fscore_l, totvar=False), axis=0)
figure(figsize=(16,4)), subplot(1,2,1), pcolor(facprop_r), colorbar()
subplot(1,2,2), pcolor(facprop_l), colorbar()
Out[131]:
In [132]:
def controlled_stride(fullstate, param0_r, param0_l, refstate_r, refstate_l, ctrl_r,
ctrl_l, facprop_r, facprop_l, addDict):
"""
This function performs a stride of the controlled SLIP. Control maps and reference
motion / values must be given.
:args:
fullstate ([k+3]x float): the initial full state of the system: [CoM state; factors]
param0_r (5x float): the reference leg parameters for right leg in
the periodic motion: [k, alpha, L0, beta, dE]
param0_l (5x float): the reference leg parameters for left leg
refstate_r (3x float): the right apex state for periodic motions:
[height, horiz. speed, lat. speed]. *NOTE* The reference value for factors is always zero.
refstate_l (3x float): the left apex state for periodic motions
ctrl_r (2D array): a map that predicts the off-reference values for SLIP parameters as linear function
of the full state (which is [CoM state; factors].T). The "right leg controller"
ctrl_l (2D array): the corresponding "left leg controller"
facprop_r (2D array): The propagator that maps the right full (apex) state to the factors at left apex.
facprop_l (2D array): The propagator that maps the left full (apex) state to the factors at right apex.
addDict (dict): contains 'm' (model mass in kg) and 'g', the gravity (typically -9.81 [ms^-2])
:returns:
final state ([k+3]x float)): the final full state of the system [CoM state; factors] after one stride.
*NOTE* it is assumed that the reference values for the "factors" are zero.
"""
# == right step ==
IC = fullstate.squeeze()[:3]
d_state_r = fullstate.squeeze() - refstate_r.squeeze()
# compute SLIP control input
d_param_r = dot(ctrl_r, d_state_r)
p_r = param0_r.squeeze() + d_param_r.squeeze()
# simulate SLIP
com_state_l = su.finalState(IC, p_r, addDict).squeeze()
# propagate factors state
facs_state_l = dot(facprop_r, d_state_r).squeeze()
fullstate_l = hstack([com_state_l, facs_state_l])
# == left step ==
IC_l = fullstate_l[:3]
d_state_l = fullstate_l - refstate_l.squeeze()
# DEBUG
#print "d state l :", d_state_l
# compute SLIP control input
d_param_l = dot(ctrl_l, d_state_l)
p_l = param0_l.squeeze() + d_param_l.squeeze()
# simulate SLIP
# DEBUG
#print "dstate r:", d_state_r
#print "dstate l:", d_state_l
#print "p: ", p_l
#print "com: ", IC_l
#return 2
com_state_final = su.finalState(IC_l, p_l, addDict).squeeze()
# propagate factors state
facs_state_final = dot(facprop_l, d_state_l).squeeze()
fullstate_final = hstack([com_state_final, facs_state_final])
# DEBUG
#print "d state final:", fullstate_final - refstate_r.squeeze()
return fullstate_final
def get_auto_sys(param0_r, param0_l, refstate_r, refstate_l, ctrl_r, ctrl_l, facprop_r, facprop_l, addDict):
"""
Returns a function f that expresses the autonomous system: x_(n+1) = f (x_n).
This is a convenience function for e.g. calculating the jacobian.
:args:
param0_r (5x float): the reference leg parameters for right leg in
the periodic motion: [k, alpha, L0, beta, dE]
param0_l (5x float): the reference leg parameters for left leg
refstate_r (3x float): the right apex state for periodic motions:
[height, horiz. speed, lat. speed]. *NOTE* The reference value for factors is always zero.
refstate_l (3x float): the left apex state for periodic motions
ctrl_r (2D array): a map that predicts the off-reference values for SLIP parameters as linear function
of the full state (which is [CoM state; factors].T). The "right leg controller"
ctrl_l (2D array): the corresponding "left leg controller"
facprop_r (2D array): The propagator that maps the right full (apex) state to the factors at left apex.
facprop_l (2D array): The propagator that maps the left full (apex) state to the factors at right apex.
addDict (dict): contains 'm' (model mass in kg) and 'g', the gravity (typically -9.81 [ms^-2])
:returns:
a lambda function f(fullstate) that calls the controlled_stride function using the given parameters.
"""
return lambda fullstate: controlled_stride(fullstate, param0_r, param0_l, refstate_r, refstate_l, ctrl_r,
ctrl_l, facprop_r, facprop_l, addDict)
In [133]:
allow_nonzero_ref = 1 # set this to zero to force "zero" as reference for the fscores!
#mean_ICl
refstate_r = hstack([ICp_r, allow_nonzero_ref * mean(fscore_r, axis=0)]) # the latter should be zero anyway
refstate_l = hstack([ICp_l, allow_nonzero_ref * mean(fscore_l, axis=0)]) # the latter should be zero anyway
f = get_auto_sys(mean_pr, mean_pl, refstate_r, refstate_l, ctrl_r, ctrl_l, facprop_r, facprop_l, {'m': mass, 'g':-9.81})
#IC = refstate_r
#stride_res = f(refstate_r + array([.001, 0, 0, 0, 0, 0,0,0]))
#print stride_res
#print stride_res - refstate_r
#print "======="
#controlled_stride(refstate_r, mean_pr, mean_pl, refstate_r, refstate_l, ctrl_r, ctrl_l, facprop_r, facprop_l, {'m': 50, 'g':-9.81})
In [134]:
print var((dt_param_l - dot (ctrl_l, dt_fstate_l.T).T), axis=0) / var(dt_param_l, axis=0)
print var((dt_param_r - dot (ctrl_r, dt_fstate_r.T).T), axis=0) / var(dt_param_r, axis=0)
In [135]:
# test sensitivity. This is bugfixing prior to computation of Jacobian.
h = .0001
elem = 0
IC = refstate_r.copy()
IC[elem] += h
fsp = f(IC)
IC[elem] -= 2*h
fsn = f(IC)
set_printoptions(precision=4)
print array(fsp - refstate_r) / (2.*h)
print array(fsn - refstate_r) / (2.*h)
In [136]:
J = []
h = .001
for elem in range(len(refstate_r)):
IC = refstate_r.copy()
IC[elem] += h
fsp = f(IC)
IC[elem] -= 2*h
fsn = f(IC)
J.append((fsp - fsn) / (2.*h))
J = vstack(J).T
In [137]:
print eig(J)[0]
figure()
for ev in eig(J)[0]:
plot(ev.real, ev.imag, 'rd')
#t = linspace(0, 2.*pi)
#plot(sin(t), cos(t), 'k--')
#plot(.5*sin(t), .5*cos(t), 'k--')
axis('equal')
vi_red.vis1()
xlim(-1,1)
ylim(-1,1)
Out[137]:
In [138]:
# compute stride maps from sections maps
# note: the correct order in the product of maps is map_n * map_(n-1) * ... * map_1
# otherwise, the resulting matrix is not a propagator
# -> reverse ordering of maps
all_maps = [reduce(dot, maps[::-1]) for maps in zip(*maps_pt)]
vi3 = ut.VisEig(127, False)
for A in all_maps:
vi3.add(eig(A)[0])
figure(figsize=(18, 6))
subplot(1,3,1)
vr = vi_full.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
xlim(-1,1)
ylim(-1,1)
title('eigenvalue distribution "reduced model" (red)\n vs. full model (sections at apex)')
subplot(1,3,2)
vr = vi3.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
xlim(-1,1)
ylim(-1,1)
title('eigenvalues of full kinematic model\nwith %i sections per stride\n vs. reduced model(red)'
% len(map_sections))
xlabel('real part')
ylabel('imaginary part')
subplot(1,3,3)
vr = vi3.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
for ev in eig(J)[0]:
plot(ev.real, ev.imag, 'kd')
xlim(-1,1)
ylim(-1,1)
title('\n'.join(['eigenvalues of full kinematic model',
'with %i sections per stride', 'vs. reduced model(red)',
'vs. forward model (diamonds)'])
% len(map_sections))
xlabel('real part')
ylabel('imaginary part')
draw()
In [139]:
mag_thresh = .33
#mag_thresh = 0
facs_l_large = array(abs(facs_l) > mag_thresh) * facs_l
facs_r_large = array(abs(facs_r) > mag_thresh) * facs_r
figure(figsize=(15,4))
subplot(2,1,1)
pcolor(facs_l_large.T)
colorbar()
clim(-1,1)
xlim(0,41)
gca().set_yticks(arange(3) + .5)
gca().set_yticklabels([])
gca().set_xticks(arange(41) + .5)
gca().set_xticklabels([])
ylabel('for left leg')
title('elements of factors for predicting SLIP parameters, thresholded')
subplot(2,1,2)
pcolor(facs_r_large.T)
colorbar()
lbls = k.selection[2:] + [('v_' + pt) for pt in k.selection[:2] + k.selection[3:]]
gca().set_xticks(arange(41) + .5)
gca().set_xticklabels(lbls, rotation=90)
gca().set_yticks(arange(3) + .5)
ylabel('for right leg')
gca().set_yticklabels([])
clim(-1,1)
xlim(0,41)
print len(lbls)
In [140]:
# test autonomy of system [IC; factor 2; factor 3]
# test autonomus systems: ankle-SLIP unilateral / bilateral
# find good reduced EXPLICIT model!
# John: looking only at differences, it should be possible to compute the jacobian without knowing the fix point.
# maybe re-compute periodic orbit w.r.t. mean IC
# can we rotate the 5-factor state to COM-state + 2 factors?